/**
 * @file GridMapCV.h
 * @author rj.wang (rj.wang@aotbot.com)
 * @brief 栅格地图 (栅格数据通过cv::Mat存储)
 * @version 0.1
 * @date 2024-11-08
 * @copyright Copyright (c) 2024
 */

#ifndef __COMMON_PLANNING_GRID_MAP_CV_H_
#define __COMMON_PLANNING_GRID_MAP_CV_H_

#include <stdint.h>
#include <sstream>
#include <string>
#include <opencv2/opencv.hpp>

#include "Pose2D.h"

namespace common {
namespace planning {

template <class T>
class GridMapCV {
private:
  //x coordinate of left-bottom grid's center in world
  double origin_center_x_{0.0}; //unit: m
  //y coordinate of left-bottom grid's center in world
  double origin_center_y_{0.0}; //unit: m
  //grid's side length
  double resolution_{0.05}; //unit: m
  //map's data
  cv::Mat data_{};
  
public:
  //set origin_center_x_, origin_center_y_
  inline void SetOrigin(const Pose& origin) {
    origin_center_x_ = origin.X();
    origin_center_y_ = origin.Y();
  }

  //set resolution_
  inline void SetResolution(const double resolution) {
    resolution_ = resolution;
  }

  //get reference of grid map's data_
  inline cv::Mat& MapData() { return data_; }

  //get data_.cols
  inline int64_t Width() const { return data_.cols; }

  //get data_.rows
  inline int64_t Height() const { return data_.rows; }

  //get resolution_
  inline double Resolution() const { return resolution_; }

  //get origin_center_x_, origin_center_y_
  inline Pose Origin() const { 
    return Pose(origin_center_x_, origin_center_y_, 0); 
  }

  //xmin of gridmap in world coordinate
  inline double GetXmin() const {
    return origin_center_x_ - 0.5 * resolution_;
  }
  
  //xmax of gridmap in world coordinate
  inline double GetXmax() const {
    return GetXmin() + data_.cols * resolution_;
  }
  
  //ymin of gridmap in world coordinate
  inline double GetYmin() const {
    return origin_center_y_ - 0.5 * resolution_;
  }
  
  //ymax of gridmap in world coordinate
  inline double GetYmax() const {
    return GetYmin() + data_.rows * resolution_;
  }
  
  //does grid(ix, iy) belong to this grid map
  inline bool IsValid(const int64_t ix, const int64_t iy) const {
    return 0 <= ix && ix < data_.cols && 0 <= iy && iy < data_.rows;
  }
  
  //transform from world coordinate x to grid map ix
  inline int64_t X2IX(const double x) const {
    return round((x - origin_center_x_) / resolution_);
  }
  
  //transform from world coordinate y to grid map iy
  inline int64_t Y2IY(const double y) const {
    return round((y - origin_center_y_) / resolution_);
  }
  
  //transform from grid map ix to world coordinate x
  inline double IX2X(const int64_t ix) const {
    return ix * resolution_ + origin_center_x_;
  }
  
  //transform from grid map iy to world coordinate y
  inline double IY2Y(const int64_t iy) const {
    return iy * resolution_ + origin_center_y_;
  }

  //get grid(ix, iy) state
  inline T GetGrid(const int64_t ix, const int64_t iy) const {
    return data_.ptr<T>(iy)[ix];
  }
  
  //set grid(ix, iy) state value
  inline void SetGrid(const int64_t ix, const int64_t iy, const T value) {
    data_.ptr<T>(iy)[ix] = value;
  }
  
};

} // namespace planning
} // namespace common

#endif // __COMMON_PLANNING_GRID_MAP_CV_H_